/*控制策略：
    如何避免干涉/碰撞？
    1、上升：上升速度（二号、三号、四号关节加快）、旋转速度慢（一号关节）；
    2、下降反之；
    3、当输入的 Z < 车身高度 时，适当（看实际情况）减小 Y轴 数值，拉开机械臂与车体距离；
    4、Y轴 调整策略：根据输入的 X 值判断，当过远时适当减小 y值；
    5、设置最小的x轴和最大的x轴，极端状况停止在最小处；

    如何设置路径？
    1、倒“U字”路径：
        确定左上，右上点，确定矩形边框，走矩形；


*/

// 1.包含头文件
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64.hpp"
#include <std_msgs/msg/float32_multi_array.hpp>
#include "std_msgs/msg/int32_multi_array.hpp"
#include <std_msgs/msg/int32.hpp>
#include <iostream>
#include <chrono>
#include <thread>
#include <cmath> // 包含三角函数和常量
#include <mutex>
#include <unistd.h> // ::sleep

// 状态枚举
enum ArmState
{
    INITIALIZING, // 初始化状态
    ASCENDING,    // 上升阶段
    TRANSVERSING, // 横向移动
    DESCENDING    // 下降阶段
};

enum State
{
    INIT,
    ASCENDING_2, // 上升阶段
    DESCENDING_2 // 下降阶段
};

enum Case3State
{
    CASE3_INIT = 0,
    CASE3_GO_TOP,    // 先走到右上/左上
    CASE3_ASCEND_2,  // 下降到底部
    CASE3_DESCEND_2, // 回到高位
};
// 控制模式
int control_mode = 0;

// 底座相对于车顶的长度
float car_high = 1200.0;
// 车宽
float car_width = 1500.0;
// 车身角与臂起始成的角度
float theta = 40 * (M_PI / 180.0f);
//
float target_z;
//
float start_high = 1500;
// 计算误差,应与joint中的误差对应
const float tolerance1 = 1;
const float tolerance2 = 10;

float error_j12, error_j22, error_j32;
float pub_x, pub_y, pub_z;

// 模式下指定清扫位置的坐标
float model_x = 0.0f;
float model_y = 0.0f;
float model_z = 0.0f;

// 3.自定义节点类；
class Arm_Constyle : public rclcpp::Node
{
public:
    Arm_Constyle() : Node("arm_constyle_node_cpp"), current_state(INITIALIZING)
    {
        // 初始化控制指令订阅
        x_length_ = this->create_subscription<std_msgs::msg::Float32MultiArray>(
            "float_array_topic", 10,
            std::bind(&Arm_Constyle::todo_callback, this, std::placeholders::_1));
        // 合并坐标订阅（原三个订阅器合并为一个）
        sub_merged_pose_ = this->create_subscription<std_msgs::msg::Float32MultiArray>(
            "merged_pose_topic", 10,
            std::bind(&Arm_Constyle::merged_pose_callback, this, std::placeholders::_1));
        state_pub = this->create_publisher<std_msgs::msg::Int32>("joint_state", 10);
        // 实例化回调组
        callback_group = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
        motionstyle_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
        //
        rclcpp::SubscriptionOptions options_style;
        options_style.callback_group = motionstyle_group;
        // --向逆解程序发布坐标
        tar_x = this->create_publisher<std_msgs::msg::Float64>("x_goat", 10);
        tar_y = this->create_publisher<std_msgs::msg::Float64>("y_goat", 10);
        tar_z = this->create_publisher<std_msgs::msg::Float64>("z_goat", 10);
        // 创建订阅方
        // 订阅控制模式
        control_mode_sub_ = this->create_subscription<std_msgs::msg::Int32>(
            "control_mode", 10,
            std::bind(&Arm_Constyle::control_mode_callback, this, std::placeholders::_1), options_style);
        // 订阅来自编码器和 ADC的数据
        this->joint1_sub = this->create_subscription<std_msgs::msg::Int32>("joint1_p", 10,
                                                                           std::bind(&Arm_Constyle::j1_cb, this, std::placeholders::_1));
        this->joint2_sub = this->create_subscription<std_msgs::msg::Float64>("joint2_p", 10,
                                                                             std::bind(&Arm_Constyle::j2_cb, this, std::placeholders::_1));
        this->joint3_sub = this->create_subscription<std_msgs::msg::Float64>("joint3_p", 10,
                                                                             std::bind(&Arm_Constyle::j3_cb, this, std::placeholders::_1));
        // 订阅逆解求得的数据，角度、行程
        this->j1_sub = this->create_subscription<std_msgs::msg::Float64>("joint1_gp", 10,
                                                                         std::bind(&Arm_Constyle::j1_icb, this, std::placeholders::_1));
        this->j2_sub = this->create_subscription<std_msgs::msg::Float64>("joint2_gp", 10,
                                                                         std::bind(&Arm_Constyle::j2_icb, this, std::placeholders::_1));
        this->j3_sub = this->create_subscription<std_msgs::msg::Float64>("joint3_gp", 10,
                                                                         std::bind(&Arm_Constyle::j3_icb, this, std::placeholders::_1));
        // 这里发布的数据是直接操作关节动作的
        j1_pub = this->create_publisher<std_msgs::msg::Float64>("joint1_gp", 10);
        j2_pub = this->create_publisher<std_msgs::msg::Float64>("joint2_gp", 10);
        j3_pub = this->create_publisher<std_msgs::msg::Float64>("joint3_gp", 10);
        j4_pub = this->create_publisher<std_msgs::msg::Int32>("joint4_gp", 10);

        // 发布阀门状态
        serial_state_pub_ = this->create_publisher<std_msgs::msg::Int32MultiArray>("big_valve_state", 10);
        endaction_pub = this->create_publisher<std_msgs::msg::Int32>("end_action", 10);
        // 发布can帧模式选择
        run_model_pub_ = this->create_publisher<std_msgs::msg::Int32>("run_model_sub", 10);
        pub_timer_ = this->create_wall_timer(std::chrono::milliseconds(50), std::bind(&Arm_Constyle::publish_message, this), callback_group);
    }

private:
    rclcpp::TimerBase::SharedPtr pub_timer_;
    rclcpp::CallbackGroup::SharedPtr callback_group;
    rclcpp::CallbackGroup::SharedPtr motionstyle_group;
    std::mutex pub_mutex_; // 保护 pub_x, pub_y, pub_z
    ArmState current_state;
    State current_state_2;
    std::mutex data_mutex_;

    // case 3 状态
    Case3State case3_state_ = CASE3_INIT;
    bool case3_new_target_sent_ = false;
    // 记录上一次逆解值，用来判断“新目标已下发”
    float case3_prev_j1_ = 9999.f;
    float case3_prev_j2_ = 9999.f;
    float case3_prev_j3_ = 9999.f;
    // -----

    // 正解得到的实时坐标值
    float real_time_x = 0.0f;
    float real_time_y = 0.0f;
    float real_time_z = 0.0f;
    float target_x = 0.0f;

    //
    float j1_cbdata = 0.0, j2_cbdata = 0.0, j3_cbdata = 0.0;
    //
    float j1_icbdata = 100.0, j2_icbdata = 100.0, j3_icbdata = 100.0;

    // -----------
    float init_prev_j1_icb = 0.0f; // 保存逆解目标的先前值
    float init_prev_j2_icb = 0.0f;
    float init_prev_j3_icb = 0.0f;
    // 新增成员变量
    float transverse_target_x = 0.0f;
    float transverse_target_y = 0.0f; // 用于保存平移阶段的Y目标
    float prev_j1_icb = 0.0f;         // 保存逆解目标的先前值
    float prev_j2_icb = 0.0f;
    float prev_j3_icb = 0.0f;
    bool new_target_processed = false; // 标志新目标是否被处理
    // 类成员变量新增
    float descending_prev_j1_icb = 0.0f;
    float descending_prev_j2_icb = 0.0f;
    float descending_prev_j3_icb = 0.0f;
    bool descending_target_processed = false;

    float backing_prev_j1_icb = 0.0f;
    float backing_prev_j2_icb = 0.0f;
    float backing_prev_j3_icb = 0.0f;

    //
    float descend2_prev_j1_icb = 0.0f;
    float descend2_prev_j2_icb = 0.0f;
    float descend2_prev_j3_icb = 0.0f;
    //
    float ascend2_prev_j1_icb = 0.0f;
    float ascend2_prev_j2_icb = 0.0f;
    float ascend2_prev_j3_icb = 0.0f;
    // ---------------

    // 目标位置发布
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr tar_x;
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr tar_y;
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr tar_z;
    // 控制模式订阅
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr control_mode_sub_;
    // 一号编码器，二三号ADC的数据
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr joint1_sub;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr joint2_sub;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr joint3_sub;
    // 逆解求得的数据，角度、行程
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr j1_sub;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr j2_sub;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr j3_sub;
    // 末端姿态确认
    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr state_pub;
    // 订阅实时位置
    rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr sub_merged_pose_;
    rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr x_length_;
    // 发布阀门状态
    rclcpp::Publisher<std_msgs::msg::Int32MultiArray>::SharedPtr serial_state_pub_;
    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr endaction_pub;
    // 发布can帧发布模式选择
    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr run_model_pub_;
    // 一号关节旋转量，g2,g3油缸行程发布
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr j1_pub;
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr j2_pub;
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr j3_pub;
    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr j4_pub;

    // 订阅控制模式
    void control_mode_callback(const std_msgs::msg::Int32::SharedPtr msg)
    {
        control_mode = msg->data;
    }
    // 来自ADC和编码器的消息
    void j1_cb(const std_msgs::msg::Int32::SharedPtr j1_msg)
    {
        j1_cbdata = 0;
        j1_cbdata = float(j1_msg->data);
    }
    void j2_cb(const std_msgs::msg::Float64::SharedPtr j2_msg)
    {
        j2_cbdata = 0;
        j2_cbdata = j2_msg->data;
    }
    void j3_cb(const std_msgs::msg::Float64::SharedPtr j3_msg)
    {
        j3_cbdata = 0;
        j3_cbdata = j3_msg->data;
    }
    // 逆解求得的数据，角度、行程
    void j1_icb(const std_msgs::msg::Float64::SharedPtr j1_imsg)
    {
        j1_icbdata = 0;
        j1_icbdata = j1_imsg->data;
    }
    void j2_icb(const std_msgs::msg::Float64::SharedPtr j2_imsg)
    {
        j2_icbdata = 0;
        j2_icbdata = j2_imsg->data;
    }
    void j3_icb(const std_msgs::msg::Float64::SharedPtr j3_imsg)
    {
        j3_icbdata = 0;
        j3_icbdata = j3_imsg->data;
    }
    // 实时坐标回调函数实现
    void merged_pose_callback(const std_msgs::msg::Float32MultiArray::SharedPtr msg)
    {
        // 数据校验
        if (msg->data.size() < 3)
        {
            RCLCPP_ERROR(this->get_logger(), "Invalid pose data size: %zu", msg->data.size());

            return;
        }

        // 同步更新坐标
        std::lock_guard<std::mutex> lock(data_mutex_);
        real_time_x = msg->data[0];
        real_time_y = msg->data[1];
        real_time_z = msg->data[2];
    }

    void todo_callback(const std_msgs::msg::Float32MultiArray::SharedPtr msg)
    {
        int temp_control = 3;
        // 阀门数据定义
        auto serial_data = std_msgs::msg::Int32MultiArray();
        auto endact_data = std_msgs::msg::Int32();
        auto run_model = std_msgs::msg::Int32();
        serial_data.data.resize(4);
        // 模式选择
        // switch (control_mode)
        switch (temp_control)
        {
        // 回归初始位置
        case 0:
        {
            // 阀门数据定义
            auto serial_data = std_msgs::msg::Int32MultiArray();
            // auto endact_data = std_msgs::msg::Int32();
            serial_data.data.resize(4);

            RCLCPP_INFO(this->get_logger(), "0-2.5扫----triggered");
            // 获取坐标快照
            float current_x;
            {
                std::lock_guard<std::mutex> lock(data_mutex_);
                current_x = real_time_x;
            }
            // 右侧距离
            float x_right = msg->data[0];
            // 左侧距离
            float x_left = msg->data[1];
            // 传来的高度
            float z_high = msg->data[2];

            // 判断y轴
            float x_min = std::min(x_right, -x_left);
            float target_y = x_min * std::tan(theta);

            if (current_x > 0)
            {
                target_x = x_right;
            }
            if (current_x < 0)
            {
                target_x = x_left;
            }
            auto joint_state = std_msgs::msg::Int32();

            // 保存逆解目标值以检测更新
            init_prev_j1_icb = j1_icbdata;
            init_prev_j2_icb = j2_icbdata;
            init_prev_j3_icb = j3_icbdata;
            // 先到右上/左上
            std::lock_guard<std::mutex> lock(pub_mutex_);
            pub_x = target_x;
            pub_y = target_y;
            pub_z = z_high;
            std::cout << "正在前往： " << target_x << " , " << target_y << " , " << z_high << "\n";
            float error_j1 = std::abs(j1_cbdata - j1_icbdata);
            float error_j2 = std::abs(j2_cbdata - j2_icbdata);
            float error_j3 = std::abs(j3_cbdata - j3_icbdata);
            // 输出一下误差
            error_j12 = std::abs(j1_cbdata - j1_icbdata);
            std::cout << "实际位置 | 逆解位置" << "\n";
            std::cout << j1_cbdata << " | " << j1_icbdata << "\n";
            error_j22 = std::abs(j2_cbdata - j2_icbdata);
            std::cout << j2_cbdata << " | " << j2_icbdata << "\n";
            error_j32 = std::abs(j3_cbdata - j3_icbdata);
            std::cout << j3_cbdata << " | " << j3_icbdata << "\n";
            std::cout << "\n";
            // // 防止检查值相同直接执行
            // if (j1_icbdata != init_prev_j1_icb || j2_icbdata != init_prev_j2_icb || j3_icbdata != init_prev_j3_icb)
            // {
            // }
            // else
            // {
            //     std::cout << "值相同" << "\n";

            //     break;
            // }
            if (error_j12 <= tolerance1 && error_j22 <= tolerance2 && error_j32 <= tolerance2)
            {
                RCLCPP_INFO(this->get_logger(), "到达预设高度......");
                joint_state.data = 0;
                state_pub->publish(joint_state);
                std::this_thread::sleep_for(std::chrono::seconds(2));
                RCLCPP_INFO(this->get_logger(), "等待结束......");
                // 一号关节回归零位
                float j1 = 2.0;
                auto j1_state = std_msgs::msg::Float64();
                j1_state.data = j1;
                j1_pub->publish(j1_state);
                if (std::abs(j1_cbdata - j1_icbdata) < tolerance2)
                {
                    // 一号关节回归零位，发送停止命令
                    auto run_model = std_msgs::msg::Int32();
                    run_model.data = 4;
                    run_model_pub_->publish(run_model);
                }
            }
            break;
        }

        // 倒U字路径
        case 1:
        {
            RCLCPP_INFO(this->get_logger(), "倒U字路径 triggered");
            // 获取坐标快照
            float current_x;
            {
                std::lock_guard<std::mutex> lock(data_mutex_);
                current_x = real_time_x;
            }
            // 右侧距离
            float x_right = msg->data[0];
            // 左侧距离
            float x_left = msg->data[1];
            // 传来的高度
            float z_high = msg->data[2];

            // 判断y轴
            float x_min = std::min(x_right, -x_left);
            float target_y = x_min * std::tan(theta);

            if (current_x > 0)
            {
                target_x = x_right;
            }
            if (current_x < 0)
            {
                target_x = x_left;
            }

            auto joint_state = std_msgs::msg::Int32();

            // 保存逆解目标值以检测更新
            init_prev_j1_icb = j1_icbdata;
            init_prev_j2_icb = j2_icbdata;
            init_prev_j3_icb = j3_icbdata;
            switch (current_state)
            {
            // 初始位置
            case INITIALIZING:
            {
                /* 初始化阶段：确定起始位置
                若此时x>0时从说明启动时在右侧，从开始，否则左侧开始 */
                if (current_x > 0)
                {
                    target_x = x_right;
                }
                else if (current_x < 0)
                {
                    target_x = x_left;
                }
                // 发布目标
                target_z = start_high;
                if (target_x == 0)
                {
                    return;
                }

                std::lock_guard<std::mutex> lock(pub_mutex_);
                pub_x = target_x;
                pub_y = target_y;
                pub_z = target_z;
                std::cout << "正在前往： " << target_x << " , " << target_y << " , " << target_z << "\n";

                float error_j1 = std::abs(j1_cbdata - j1_icbdata);
                float error_j2 = std::abs(j2_cbdata - j2_icbdata);
                float error_j3 = std::abs(j3_cbdata - j3_icbdata);
                error_j12 = std::abs(j1_cbdata - j1_icbdata);
                std::cout << "实际位置 | 逆解位置" << "\n";
                std::cout << j1_cbdata << " | " << j1_icbdata << "\n";
                error_j22 = std::abs(j2_cbdata - j2_icbdata);
                std::cout << j2_cbdata << " | " << j2_icbdata << "\n";
                error_j32 = std::abs(j3_cbdata - j3_icbdata);
                std::cout << j3_cbdata << " | " << j3_icbdata << "\n";
                std::cout << "\n";
                // 防止检查值相同直接执行
                // if (j1_icbdata != init_prev_j1_icb || j2_icbdata != init_prev_j2_icb || j3_icbdata != init_prev_j3_icb)
                // {
                // }
                // else
                // {
                //     break;
                // }
                if (error_j1 <= tolerance1 && error_j2 <= tolerance2 && error_j3 <= tolerance2)
                {
                    current_state = ASCENDING;
                    // 已到达右下或左下
                    RCLCPP_INFO(this->get_logger(), "到达初始位置......");
                    //
                    // joint_state.data = 1;
                    // state_pub->publish(joint_state);
                    //
                    std::this_thread::sleep_for(std::chrono::seconds(2));
                    RCLCPP_INFO(this->get_logger(), "等待结束......");
                }
                break;
            }
            // 上升
            case ASCENDING:
            {
                RCLCPP_INFO(this->get_logger(), "开始上升......");
                // 发布阀门状态
                serial_data.data[0] = serial_data.data[1] = 1;
                serial_data.data[2] = serial_data.data[3] = 2;
                endact_data.data = 1;
                // 低压阀门  高压阀门
                serial_state_pub_->publish(serial_data);
                // 扫刷开启
                endaction_pub->publish(endact_data);
                std::cout << "阀门及扫刷状态已发布" << std::endl;
                // 开启两个泵
                run_model.data = 3;
                run_model_pub_->publish(run_model);

                // 保持当前X轴，上升到安全高度
                // 确定平移阶段的目标
                if (target_x > 0)
                {
                    transverse_target_x = x_left; // 右侧转左侧
                                                  // 计算并保存对应的y值
                    float x_min_trans = std::min(x_right, -x_left);
                    transverse_target_y = x_min_trans * std::tan(theta);
                }
                else
                {
                    transverse_target_x = x_right; // 左侧转右侧
                    float x_min_trans = std::min(x_right, -x_left);
                    transverse_target_y = x_min_trans * std::tan(theta);
                }
                // 保存逆解目标值以检测更新
                prev_j1_icb = j1_icbdata;
                prev_j2_icb = j2_icbdata;
                prev_j3_icb = j3_icbdata;
                new_target_processed = false;
                // 发送右上或左上坐标
                target_z = z_high;

                std::lock_guard<std::mutex> lock(pub_mutex_);
                pub_x = target_x;
                pub_y = target_y;
                pub_z = target_z;

                std::cout << "正在前往： " << target_x << " , " << target_y << " , " << target_z << "\n";

                // 末端关节状态--1：清扫两侧
                joint_state.data = 1;
                state_pub->publish(joint_state);
                std::cout << "末端关节状态已发布：清扫两侧" << "\n";
                //
                std::cout << "实际位置 | 逆解位置" << "\n";
                error_j12 = std::abs(j1_cbdata - j1_icbdata);
                std::cout << j1_cbdata << " | " << j1_icbdata << "\n";
                error_j22 = std::abs(j2_cbdata - j2_icbdata);
                std::cout << j2_cbdata << " | " << j2_icbdata << "\n";
                error_j32 = std::abs(j3_cbdata - j3_icbdata);
                std::cout << j3_cbdata << " | " << j3_icbdata << "\n";
                std::cout << "\n";

                if (j1_icbdata != backing_prev_j1_icb || j2_icbdata != backing_prev_j2_icb || j3_icbdata != backing_prev_j3_icb)
                {
                }
                else
                {
                    break;
                }
                if (error_j12 <= tolerance1 && error_j22 <= tolerance2 && error_j32 <= tolerance2)
                {
                    current_state = TRANSVERSING;
                    RCLCPP_INFO(this->get_logger(), "到达预设高度......");
                    joint_state.data = 0;
                    state_pub->publish(joint_state);
                    std::this_thread::sleep_for(std::chrono::seconds(2));
                    RCLCPP_INFO(this->get_logger(), "等待结束......");
                }
                break;
            }

            case TRANSVERSING:
            {
                RCLCPP_INFO(this->get_logger(), "开始平移......");
                // 横向移动到对侧（形成倒U的顶部横移）
                // 使用预先设置的对侧目标
                target_x = transverse_target_x;
                // 发送左上或右上坐标
                // 注意：每次回调都会重新计算target_y，此处使用最新的x_min
                target_z = z_high;

                std::lock_guard<std::mutex> lock(pub_mutex_);
                pub_x = target_x;
                pub_y = target_y; // target_y已根据新x_min更新
                pub_z = target_z;

                std::cout << "正在前往： " << target_x << " , " << target_y << " , " << target_z << "\n";

                // 等待新目标被处理
                if (!new_target_processed)
                {
                    if (j1_icbdata != prev_j1_icb || j2_icbdata != prev_j2_icb || j3_icbdata != prev_j3_icb)
                    {
                        new_target_processed = true;
                    }
                    else
                    {
                        break; // 继续等待
                    }
                }

                float error_j1 = std::abs(j1_cbdata - j1_icbdata);
                float error_j2 = std::abs(j2_cbdata - j2_icbdata);
                float error_j3 = std::abs(j3_cbdata - j3_icbdata);
                //
                descending_prev_j1_icb = j1_icbdata;
                descending_prev_j2_icb = j2_icbdata;
                descending_prev_j3_icb = j3_icbdata;
                descending_target_processed = false;
                //
                std::cout << "实际位置 | 逆解位置" << "\n";
                error_j12 = std::abs(j1_cbdata - j1_icbdata);
                std::cout << j1_cbdata << " | " << j1_icbdata << "\n";
                error_j22 = std::abs(j2_cbdata - j2_icbdata);
                std::cout << j2_cbdata << " | " << j2_icbdata << "\n";
                error_j32 = std::abs(j3_cbdata - j3_icbdata);
                std::cout << j3_cbdata << " | " << j3_icbdata << "\n";
                std::cout << "\n";
                if (error_j1 <= tolerance1 && error_j2 <= tolerance2 && error_j3 <= tolerance2)
                {
                    current_state = DESCENDING;
                    RCLCPP_INFO(this->get_logger(), "到达对侧目标......");
                    //
                    joint_state.data = 1;
                    state_pub->publish(joint_state);
                    //
                    std::this_thread::sleep_for(std::chrono::seconds(2));
                    RCLCPP_INFO(this->get_logger(), "等待结束......");
                }
                break;
            }

            case DESCENDING:
            {
                RCLCPP_INFO(this->get_logger(), "开始下降......");

                // 保持当前X轴，下降到底部
                target_z = start_high;

                std::lock_guard<std::mutex> lock(pub_mutex_);
                pub_x = target_x;
                pub_y = target_y;
                pub_z = target_z;

                std::cout << "正在前往： " << target_x << " , " << target_y << " , " << target_z << "\n";

                if (!descending_target_processed)
                {
                    if (descending_prev_j1_icb != j1_icbdata || descending_prev_j2_icb != j2_icbdata || descending_prev_j3_icb != j3_icbdata)
                    {
                        descending_target_processed = true;
                    }
                    else
                        break;
                }

                // 末端关节状态--1：清扫两侧
                joint_state.data = 1;
                state_pub->publish(joint_state);
                std::cout << "末端关节状态已发布：清扫两侧" << "\n";

                float error_j1 = std::abs(j1_cbdata - j1_icbdata);
                float error_j2 = std::abs(j2_cbdata - j2_icbdata);
                float error_j3 = std::abs(j3_cbdata - j3_icbdata);

                backing_prev_j1_icb = j1_icbdata;
                backing_prev_j2_icb = j2_icbdata;
                backing_prev_j3_icb = j3_icbdata;

                std::cout << "实际位置 | 逆解位置" << "\n";
                error_j12 = std::abs(j1_cbdata - j1_icbdata);
                std::cout << j1_cbdata << " | " << j1_icbdata << "\n";
                error_j22 = std::abs(j2_cbdata - j2_icbdata);
                std::cout << j2_cbdata << " | " << j2_icbdata << "\n";
                error_j32 = std::abs(j3_cbdata - j3_icbdata);
                std::cout << j3_cbdata << " | " << j3_icbdata << "\n";
                std::cout << "\n";

                if (error_j1 <= tolerance1 && error_j2 <= tolerance2 && error_j3 <= tolerance2)
                {
                    current_state = INITIALIZING;        // 完成循环
                    descending_target_processed = false; // 重置标志
                    std::this_thread::sleep_for(std::chrono::seconds(2));
                    RCLCPP_INFO(this->get_logger(), "等待结束......");
                    RCLCPP_INFO(this->get_logger(), "回程开始......");
                }
                break;
            }
            }
            break;
        }
        // 到达指定地点 
        case 2:
        {
            RCLCPP_INFO(this->get_logger(), "------------------到达指定地点--------------------- ");
            // 目标坐标不为0，表示接收到目标坐标
            std::lock_guard<std::mutex> lock(pub_mutex_);
            pub_x = model_x;
            pub_y = model_y;
            pub_z = model_z;

            break;
        }
        // 0-2.5扫  重写状态机 高压泵 高压阀
        case 3:
        {
            // 阀门数据定义
            auto serial_data = std_msgs::msg::Int32MultiArray();
            // auto endact_data = std_msgs::msg::Int32();
            serial_data.data.resize(4);

            RCLCPP_INFO(this->get_logger(), "0-2.5扫----triggered");
            // 获取坐标快照
            float current_x;
            {
                std::lock_guard<std::mutex> lock(data_mutex_);
                current_x = real_time_x;
            }
            // 右侧距离
            float x_right = msg->data[0];
            // 左侧距离
            float x_left = msg->data[1];
            // 传来的高度
            float z_high = msg->data[2];

            // 判断y轴
            float x_min = std::min(x_right, -x_left);
            float target_y = x_min * std::tan(theta);

            if (current_x > 0)
            {
                target_x = x_right;
            }
            if (current_x < 0)
            {
                target_x = x_left;
            }
            auto joint_state = std_msgs::msg::Int32();

            /*---------- 2. 状态机 ----------*/
            switch (case3_state_)
            {
            /*---------------------------------
             *  第一次进入：走到右上 / 左上
             *--------------------------------*/
            case CASE3_INIT:
            {
                RCLCPP_INFO(this->get_logger(), "CASE3_INIT → 去顶部");
                // 发布目标
                {
                    std::lock_guard<std::mutex> lock(pub_mutex_);
                    pub_x = target_x;
                    pub_y = target_y;
                    pub_z = z_high;
                }
                case3_new_target_sent_ = false;
                case3_state_ = CASE3_GO_TOP;
                break;
            }

            /*---------------------------------
             *  等待到达顶部 → 打开阀门/泵
             *--------------------------------*/
            case CASE3_GO_TOP:
            {
                float err1 = std::abs(j1_cbdata - j1_icbdata);
                float err2 = std::abs(j2_cbdata - j2_icbdata);
                float err3 = std::abs(j3_cbdata - j3_icbdata);

                if (!case3_new_target_sent_)
                {
                    if (j1_icbdata != case3_prev_j1_ ||
                        j2_icbdata != case3_prev_j2_ ||
                        j3_icbdata != case3_prev_j3_)
                        case3_new_target_sent_ = true; // 逆解已更新
                }

                if (case3_new_target_sent_ &&
                    err1 <= tolerance1 &&
                    err2 <= tolerance2 &&
                    err3 <= tolerance2)
                {
                    RCLCPP_INFO(this->get_logger(), "CASE3 到达顶部 → 打开阀门/泵");

                    // 阀门 & 泵
                    auto serial_data = std_msgs::msg::Int32MultiArray();
                    serial_data.data.resize(4);
                    serial_data.data[0] = serial_data.data[1] = 0; // 低压关
                    serial_data.data[2] = serial_data.data[3] = 2; // 高压开
                    auto endact_data = std_msgs::msg::Int32();
                    endact_data.data = 1; // 扫刷开
                    serial_state_pub_->publish(serial_data);
                    endaction_pub->publish(endact_data);

                    auto run_model = std_msgs::msg::Int32();
                    run_model.data = 2; // 高压泵
                    run_model_pub_->publish(run_model);

                    // 准备下降
                    case3_prev_j1_ = j1_icbdata;
                    case3_prev_j2_ = j2_icbdata;
                    case3_prev_j3_ = j3_icbdata;
                    case3_new_target_sent_ = false;
                    case3_state_ = CASE3_ASCEND_2;
                }
                break;
            }

            /*---------------------------------
             *  下降到底部
             *--------------------------------*/
            case CASE3_ASCEND_2:
            {
                // 发新目标
                {
                    std::lock_guard<std::mutex> lock(pub_mutex_);
                    pub_x = target_x;
                    pub_y = target_y;
                    pub_z = 1900.0f; // 底部
                }
                joint_state.data = 1; // 清扫姿态
                state_pub->publish(joint_state);

                float err1 = std::abs(j1_cbdata - j1_icbdata);
                float err2 = std::abs(j2_cbdata - j2_icbdata);
                float err3 = std::abs(j3_cbdata - j3_icbdata);

                if (!case3_new_target_sent_)
                {
                    if (j1_icbdata != case3_prev_j1_ ||
                        j2_icbdata != case3_prev_j2_ ||
                        j3_icbdata != case3_prev_j3_)
                        case3_new_target_sent_ = true;
                }

                if (case3_new_target_sent_ &&
                    err1 <= tolerance1 &&
                    err2 <= tolerance2 &&
                    err3 <= tolerance2)
                {
                    RCLCPP_INFO(this->get_logger(), "CASE3 下降完成 → 准备上升");
                    case3_prev_j1_ = j1_icbdata;
                    case3_prev_j2_ = j2_icbdata;
                    case3_prev_j3_ = j3_icbdata;
                    case3_new_target_sent_ = false;
                    case3_state_ = CASE3_DESCEND_2;
                }
                break;
            }

            /*---------------------------------
             *  回到高位
             *--------------------------------*/
            case CASE3_DESCEND_2:
            {
                {
                    std::lock_guard<std::mutex> lock(pub_mutex_);
                    pub_x = target_x;
                    pub_y = target_y;
                    pub_z = z_high;
                }
                joint_state.data = 1;
                state_pub->publish(joint_state);

                float err1 = std::abs(j1_cbdata - j1_icbdata);
                float err2 = std::abs(j2_cbdata - j2_icbdata);
                float err3 = std::abs(j3_cbdata - j3_icbdata);

                if (!case3_new_target_sent_)
                {
                    if (j1_icbdata != case3_prev_j1_ ||
                        j2_icbdata != case3_prev_j2_ ||
                        j3_icbdata != case3_prev_j3_)
                        case3_new_target_sent_ = true;
                }

                if (case3_new_target_sent_ &&
                    err1 <= tolerance1 &&
                    err2 <= tolerance2 &&
                    err3 <= tolerance2)
                {
                    RCLCPP_INFO(this->get_logger(), "CASE3 上升完成 → 下一轮下降");
                    // 循环往复：再回到底部
                    case3_prev_j1_ = j1_icbdata;
                    case3_prev_j2_ = j2_icbdata;
                    case3_prev_j3_ = j3_icbdata;
                    case3_new_target_sent_ = false;
                    case3_state_ = CASE3_ASCEND_2;
                }
                break;
            }
            } // end switch(case3_state_)
            break;
        }
        // 2.5-3.5扫 重写状态机 高压泵 高压阀
        case 4:
        {
            break;
        }
        // 急停
        case 5:
        {

            break;
        }

        default:
            break;
        }
    }

    void publish_message()
    {
        auto pubx = std_msgs::msg::Float64();
        auto puby = std_msgs::msg::Float64();
        auto pubz = std_msgs::msg::Float64();
        {
            // 加锁自动解锁
            std::lock_guard<std::mutex> lock(pub_mutex_);
            pubx.data = pub_x;
            puby.data = pub_y;
            pubz.data = pub_z;
            tar_x->publish(pubx);
            tar_y->publish(puby);
            tar_z->publish(pubz);
        }
    }
};

int main(int argc, char const *argv[])
{
    // 2.初始化ROS2客户端；
    rclcpp::init(argc, argv);
    auto node = std::make_shared<Arm_Constyle>();
    // 4.调用spain函数，并传入节点对象指针；
    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(node);
    executor.spin();
    // 5.资源释放
    rclcpp::shutdown();
    return 0;
}
